/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : motor.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Oct 23, 2020
  ******************************************************************************
  */
/* USER CODE END Header */

#include "motor.hpp"
#include "string.h"

MOTOR motor(TIM1,(char *)"MOTOR");

void DMA2_Stream3_IRQHandler(void)
{
	if(LL_DMA_IsActiveFlag_TC3(DMA2))
	{
		LL_DMA_DisableStream(DMA2, LL_DMA_STREAM_3);
		LL_DMA_ClearFlag_TC3(DMA2);
		LL_DMA_ClearFlag_DME3(DMA2);
		LL_DMA_ClearFlag_FE3(DMA2);
		LL_DMA_ClearFlag_HT3(DMA2);
		LL_DMA_ClearFlag_TE3(DMA2);

		LL_DMA_DisableIT_TC(DMA2, LL_DMA_STREAM_3);
		LL_TIM_DisableDMAReq_CC1(TIM1);
		LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH1);

		if(LL_DMA_GetDataTransferDirection(DMA2, LL_DMA_STREAM_3) == LL_DMA_DIRECTION_MEMORY_TO_PERIPH)
		{
			motor.DMA_TC_FLAG_NUM++;
			if(motor.DMA_TC_FLAG_NUM==4)
			{
				LL_TIM_DisableAllOutputs(TIM1);
				TIM1->ARR = 65535;

			}

			LL_DMA_SetDataTransferDirection(DMA2, LL_DMA_STREAM_3, LL_DMA_DIRECTION_PERIPH_TO_MEMORY);
			LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_3, (uint32_t)motor.erpm_Pwm_In[0]);
			LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_3, ESC_ERPM_BUFFER_LEN);

			LL_TIM_IC_SetActiveInput(TIM1, LL_TIM_CHANNEL_CH1, LL_TIM_ACTIVEINPUT_DIRECTTI);
			LL_TIM_IC_SetPrescaler(TIM1, LL_TIM_CHANNEL_CH1, LL_TIM_ICPSC_DIV1);
			LL_TIM_IC_SetFilter(TIM1, LL_TIM_CHANNEL_CH1, LL_TIM_IC_FILTER_FDIV1_N4);
			LL_TIM_IC_SetPolarity(TIM1, LL_TIM_CHANNEL_CH1, LL_TIM_IC_POLARITY_BOTHEDGE);

			LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_3);
			LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_3);
			LL_TIM_EnableDMAReq_CC1(TIM1);
			LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1);
		}
	}
}

void DMA2_Stream2_IRQHandler(void)
{
	if(LL_DMA_IsActiveFlag_TC2(DMA2))
	{
		LL_DMA_DisableStream(DMA2, LL_DMA_STREAM_2);
		LL_DMA_ClearFlag_TC2(DMA2);
		LL_DMA_ClearFlag_DME2(DMA2);
		LL_DMA_ClearFlag_FE2(DMA2);
		LL_DMA_ClearFlag_HT2(DMA2);
		LL_DMA_ClearFlag_TE2(DMA2);

		LL_DMA_DisableIT_TC(DMA2, LL_DMA_STREAM_2);
		LL_TIM_DisableDMAReq_CC2(TIM1);
		LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH2);

		if(LL_DMA_GetDataTransferDirection(DMA2, LL_DMA_STREAM_2) == LL_DMA_DIRECTION_MEMORY_TO_PERIPH)
		{
			motor.DMA_TC_FLAG_NUM++;
			if(motor.DMA_TC_FLAG_NUM==4)
			{
				LL_TIM_DisableAllOutputs(TIM1);
				TIM1->ARR = 65535;

			}

			LL_DMA_SetDataTransferDirection(DMA2, LL_DMA_STREAM_2, LL_DMA_DIRECTION_PERIPH_TO_MEMORY);
			LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_2, (uint32_t)motor.erpm_Pwm_In[1]);
			LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_2, ESC_ERPM_BUFFER_LEN);

			LL_TIM_IC_SetActiveInput(TIM1, LL_TIM_CHANNEL_CH2, LL_TIM_ACTIVEINPUT_DIRECTTI);
			LL_TIM_IC_SetPrescaler(TIM1, LL_TIM_CHANNEL_CH2, LL_TIM_ICPSC_DIV1);
			LL_TIM_IC_SetFilter(TIM1, LL_TIM_CHANNEL_CH2, LL_TIM_IC_FILTER_FDIV1_N4);
			LL_TIM_IC_SetPolarity(TIM1, LL_TIM_CHANNEL_CH2, LL_TIM_IC_POLARITY_BOTHEDGE);

			LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_2);
			LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_2);
			LL_TIM_EnableDMAReq_CC2(TIM1);
			LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2);
		}

	}

}

void DMA2_Stream6_IRQHandler(void)
{
	if(LL_DMA_IsActiveFlag_TC6(DMA2))
	{
		LL_DMA_DisableStream(DMA2, LL_DMA_STREAM_6);
		LL_DMA_ClearFlag_TC6(DMA2);
		LL_DMA_ClearFlag_DME6(DMA2);
		LL_DMA_ClearFlag_FE6(DMA2);
		LL_DMA_ClearFlag_HT6(DMA2);
		LL_DMA_ClearFlag_TE6(DMA2);

		LL_DMA_DisableIT_TC(DMA2, LL_DMA_STREAM_6);
		LL_TIM_DisableDMAReq_CC3(TIM1);
		LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH3);

		if(LL_DMA_GetDataTransferDirection(DMA2, LL_DMA_STREAM_6) == LL_DMA_DIRECTION_MEMORY_TO_PERIPH)
		{
			motor.DMA_TC_FLAG_NUM++;
			if(motor.DMA_TC_FLAG_NUM==4)
			{
				LL_TIM_DisableAllOutputs(TIM1);
				TIM1->ARR = 65535;

			}

			LL_DMA_SetDataTransferDirection(DMA2, LL_DMA_STREAM_6, LL_DMA_DIRECTION_PERIPH_TO_MEMORY);
			LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_6, (uint32_t)motor.erpm_Pwm_In[2]);
			LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_6, ESC_ERPM_BUFFER_LEN);

			LL_TIM_IC_SetActiveInput(TIM1, LL_TIM_CHANNEL_CH3, LL_TIM_ACTIVEINPUT_DIRECTTI);
			LL_TIM_IC_SetPrescaler(TIM1, LL_TIM_CHANNEL_CH3, LL_TIM_ICPSC_DIV1);
			LL_TIM_IC_SetFilter(TIM1, LL_TIM_CHANNEL_CH3, LL_TIM_IC_FILTER_FDIV1_N4);
			LL_TIM_IC_SetPolarity(TIM1, LL_TIM_CHANNEL_CH3, LL_TIM_IC_POLARITY_BOTHEDGE);

			LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_6);
			LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_6);
			LL_TIM_EnableDMAReq_CC3(TIM1);
			LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3);
		}

	}

}

void DMA2_Stream4_IRQHandler(void)
{
	if(LL_DMA_IsActiveFlag_TC4(DMA2))
	{
		LL_DMA_DisableStream(DMA2, LL_DMA_STREAM_4);
		LL_DMA_ClearFlag_TC4(DMA2);
		LL_DMA_ClearFlag_DME4(DMA2);
		LL_DMA_ClearFlag_FE4(DMA2);
		LL_DMA_ClearFlag_HT4(DMA2);
		LL_DMA_ClearFlag_TE4(DMA2);

		LL_DMA_DisableIT_TC(DMA2, LL_DMA_STREAM_4);
		LL_TIM_DisableDMAReq_CC4(TIM1);
		LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH4);


		if(LL_DMA_GetDataTransferDirection(DMA2, LL_DMA_STREAM_4) == LL_DMA_DIRECTION_MEMORY_TO_PERIPH)
		{
			motor.DMA_TC_FLAG_NUM++;
			if(motor.DMA_TC_FLAG_NUM==4)
			{
				LL_TIM_DisableAllOutputs(TIM1);
				TIM1->ARR = 65535;

			}

			LL_DMA_SetDataTransferDirection(DMA2, LL_DMA_STREAM_4, LL_DMA_DIRECTION_PERIPH_TO_MEMORY);
			LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_4, (uint32_t)motor.erpm_Pwm_In[3]);
			LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_4, ESC_ERPM_BUFFER_LEN);

			LL_TIM_IC_SetActiveInput(TIM1, LL_TIM_CHANNEL_CH4, LL_TIM_ACTIVEINPUT_DIRECTTI);
			LL_TIM_IC_SetPrescaler(TIM1, LL_TIM_CHANNEL_CH4, LL_TIM_ICPSC_DIV1);
			LL_TIM_IC_SetFilter(TIM1, LL_TIM_CHANNEL_CH4, LL_TIM_IC_FILTER_FDIV1_N4);
			LL_TIM_IC_SetPolarity(TIM1, LL_TIM_CHANNEL_CH4, LL_TIM_IC_POLARITY_BOTHEDGE);

			LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_4);
			LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_4);
			LL_TIM_EnableDMAReq_CC4(TIM1);
			LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH4);
		}

	}

}

void MOTOR::motor_Init(void)
{
	Sta = STA_INI;
	Err = ERR_NONE;
	requestTelemetry = 0;
	UnLock = false;
	executionTime_us = 0;
	cycleTime_us = 0;
	DMA_TC_FLAG_NUM = 0;
	for(uint8_t i=0;i<PWM_OUT_NUM;i++)
	{
		PWM[i] = INI_PWM;
	}
	for(uint8_t j = 0;j < PWM_OUT_NUM;j++)
	{
		for(uint8_t i = 0;i < ESC_CMD_BUFFER_LEN;i++)
		{
			ESC_CMD[j][i] = 0;
		}
		for(uint8_t i = 0;i < ESC_ERPM_BUFFER_LEN;i++)
		{
			erpm_Pwm_In[j][i] = 0;
		}
	}


	TIM_OC_InitStruct = {0};
	TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM2;
	TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE;
	TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE;
	TIM_OC_InitStruct.CompareValue = 0;
	TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH;
	TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH;
	TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW;
	TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW;

	osDelay(200);
	//启动定时器
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_3, ESC_CMD_BUFFER_LEN);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_3, (uint32_t)ESC_CMD[0]);
	LL_DMA_SetPeriphAddress(DMA2, LL_DMA_STREAM_3, (uint32_t)&(htimA->CCR1));
	LL_DMA_ClearFlag_TC3(DMA2);
	LL_DMA_ClearFlag_DME3(DMA2);
	LL_DMA_ClearFlag_FE3(DMA2);
	LL_DMA_ClearFlag_HT3(DMA2);
	LL_DMA_ClearFlag_TE3(DMA2);
	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_3);
	LL_TIM_EnableDMAReq_CC1(htimA);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH1);

	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_2, ESC_CMD_BUFFER_LEN);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_2, (uint32_t)ESC_CMD[1]);
	LL_DMA_SetPeriphAddress(DMA2, LL_DMA_STREAM_2, (uint32_t)&(htimA->CCR2));
	LL_DMA_ClearFlag_TC2(DMA2);
	LL_DMA_ClearFlag_DME2(DMA2);
	LL_DMA_ClearFlag_FE2(DMA2);
	LL_DMA_ClearFlag_HT2(DMA2);
	LL_DMA_ClearFlag_TE2(DMA2);
	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_2);
	LL_TIM_EnableDMAReq_CC2(htimA);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH2);

	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_6, ESC_CMD_BUFFER_LEN);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_6, (uint32_t)ESC_CMD[2]);
	LL_DMA_SetPeriphAddress(DMA2, LL_DMA_STREAM_6, (uint32_t)&(htimA->CCR3));
	LL_DMA_ClearFlag_TC6(DMA2);
	LL_DMA_ClearFlag_DME6(DMA2);
	LL_DMA_ClearFlag_FE6(DMA2);
	LL_DMA_ClearFlag_HT6(DMA2);
	LL_DMA_ClearFlag_TE6(DMA2);
	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_6);
	LL_TIM_EnableDMAReq_CC3(htimA);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH3);

	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_4, ESC_CMD_BUFFER_LEN);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_4, (uint32_t)ESC_CMD[3]);
	LL_DMA_SetPeriphAddress(DMA2, LL_DMA_STREAM_4, (uint32_t)&(htimA->CCR4));
	LL_DMA_ClearFlag_TC4(DMA2);
	LL_DMA_ClearFlag_DME4(DMA2);
	LL_DMA_ClearFlag_FE4(DMA2);
	LL_DMA_ClearFlag_HT4(DMA2);
	LL_DMA_ClearFlag_TE4(DMA2);
	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_4);
	LL_TIM_EnableDMAReq_CC4(htimA);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH4);


	LL_TIM_EnableAllOutputs(htimA);
	LL_TIM_CC_SetDMAReqTrigger(htimA, LL_TIM_CCDMAREQUEST_CC);


	LL_TIM_EnableCounter(htimA);


}

void MOTOR::motor_Start(void)
{

	osDelay(2274);

	for(uint8_t i=0;i<PWM_OUT_NUM;i++)
	{
		PWM[i] = INI_PWM;
	}

	for(uint16_t i=0;i<1200;i++)
	{
		motor_run();
		osDelay(1);
	}

	Sta = STA_RUN;
}

void MOTOR::motor_run(void)
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;


	for(uint8_t i=0; i<PWM_OUT_NUM; i++)
	{
		uint8_t index = 0;
		uint8_t m =0,n_L=0,n_H = 0;
		int8_t deltaTL = 0,deltaTH = 0;
		uint32_t value = 0xffffffff;
		while((erpm_Pwm_In[i][2*index+1] != 0) || ((2*index+2)<19))
		{
			m = m + n_L + n_H;
			//下降沿到上升沿之间为低电平，通过时间间隔计算连续低电平个数，一个电平时间约为22us~23us
			deltaTL = erpm_Pwm_In[i][2*index+1] - erpm_Pwm_In[i][2*index];
			n_L = (deltaTL>30)?((deltaTL>55)?3:2):1;
			//上升沿到下降沿之间为高电平，通过时间间隔计算连续高电平个数，一个电平时间约为22us~23us
			deltaTH = erpm_Pwm_In[i][2*index+2] - erpm_Pwm_In[i][2*index+1];
			n_H = (deltaTH>10)?((deltaTH>30)?((deltaTH>55)?3:2):1):0;	//如果n_H等于0，说明已经计算结束
			//因为value的初始值为0xffffffff，所以只需要将低电平的那些位置位0即可
			switch(n_L)
			{
			case 1:
				value = value & ~((uint32_t)1<<(20-m));
				break;
			case 2:
				value = value & ~((uint32_t)3<<(19-m));
				break;
			case 3:
				value = value & ~((uint32_t)7<<(18-m));
				break;
			}


			index = index +1;

		}
		erpm_Code_Edge[i] = value & 0x001fffff;		//只取后21bits

		//转成GCR码
		uint32_t codeValue = 0;
		for(uint8_t j=0; j<20; j++)
		{
			if((((erpm_Code_Edge[i]>>(20-j))^(erpm_Code_Edge[i]>>(19-j)))&0x00000001) == 1)	//从左边的第二位开始，依次与其前一位比较，如果电平有变化，则该位为1，否则为0
			{
				codeValue |= 1<<(19-j);
			}
		}

		erpm_Code_GCR[i] = codeValue;


		// First bit is start bit so discard it.
		erpm_Code_GCR[i] &= 0xfffff;		//取后面20bits

		//将GCR码转成16位格式(eeem mmmm mmmm 4bit_CRC)
		uint32_t decodedValue = decode[erpm_Code_GCR[i] & 0x1f];
		decodedValue |= decode[(erpm_Code_GCR[i] >> 5) & 0x1f] << 4;
		decodedValue |= decode[(erpm_Code_GCR[i] >> 10) & 0x1f] << 8;
		decodedValue |= decode[(erpm_Code_GCR[i] >> 15) & 0x1f] << 12;

		//crc校验
		uint32_t ccsum = decodedValue>>4;
		ccsum = ccsum ^ (decodedValue >> 8); // xor bytes
		ccsum = ccsum ^ (decodedValue >> 12); // xor nibbles
		ccsum = ~ccsum;

		if ((ccsum & 0xf) != (decodedValue & 0xf) || decodedValue > 0xffff)	//CRC校验不成功
		{

		}
		else//CRC校验成功
		{

			erpm_Code[i] = decodedValue >> 4;	//去掉CRC校验码，取前12bits
			if (erpm_Code[i] == 0x0fff)
			{
				rpm[i] = 0.0f;
			}
			else
			{
				// Convert erpm_Code to 16 bit from the GCR telemetry format (eeem mmmm mmmm)
				erpm_us[i] = (erpm_Code[i] & 0x000001ff) << ((erpm_Code[i] & 0xfffffe00) >> 9);
				// Convert period to erpm * 100
				erpm[i] = (1000000 * 60 / 100 + erpm_us[i] / 2) / erpm_us[i];
				// Convert erpm*100 to rpm
				rpm[i] = (float)erpm[i]*100.0f/NUM_OF_MOTOR_POLE_PAIR;
			}
			motorRpm.motor_rpm[i] = rpm[i];
		}


	}
	memset(erpm_Pwm_In,0,sizeof(erpm_Pwm_In));	//将接受跳边沿时间的数组重新置为0

	xQueueOverwrite(queueMotorRpm,&motorRpm);


	if(Sta == STA_RUN)
	{
		xQueuePeek(queueRCCommand, &rcCommand, 0);
		xQueuePeek(queueMotorPWM,&motorPWM, 0);
		if(rcCommand.Key[3]<1) UnLock = false;
		else if(rcCommand.Key[3]>1) UnLock = true;
	}
	if(UnLock == false)
	{
		for(uint8_t i=0;i<PWM_OUT_NUM;i++)
		{
			PWM[i] = INI_PWM;
		}
	}
	else
	{
		for(uint8_t i=0;i<PWM_OUT_NUM;i++)
		{
			PWM[i] = rcCommand.Thr;
			if(PWM[i]<50)
				PWM[i] = 0;
		}
	}

	for(uint8_t i = 0;i < PWM_OUT_NUM;i++)
	{
		if(PWM[i]>1750) PWM[i] = 1750;
		uint16_t packet = (PWM[i]<<1)|(requestTelemetry?1:0);
		// compute checksum
		int csum = 0;
		int csum_data = packet;
		for (int k = 0; k < 3; k++) {
			csum ^=  csum_data;   // xor data by nibbles
			csum_data >>= 4;
		}
		csum = ~csum;
		csum &= 0xf;
		// append checksum
			packet = (packet << 4) | csum;
		// generate pulses for whole packet
		for (uint8_t j = 0; j < 16; j++)
			{
			ESC_CMD[i][j] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0;  // MSB first
			packet <<= 1;
		}
	}

	DMA_TC_FLAG_NUM = 0;

	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1);
	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2);
	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3);
	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4);
	TIM1->ARR = 29;
	TIM1->CNT = 0;


	LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct);
	LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct);
	LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct);
	LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH4, &TIM_OC_InitStruct);

	LL_DMA_DisableStream(DMA2, LL_DMA_STREAM_3);
	LL_DMA_ClearFlag_TC3(DMA2);
	LL_DMA_ClearFlag_DME3(DMA2);
	LL_DMA_ClearFlag_FE3(DMA2);
	LL_DMA_ClearFlag_HT3(DMA2);
	LL_DMA_ClearFlag_TE3(DMA2);
	LL_DMA_SetDataTransferDirection(DMA2, LL_DMA_STREAM_3, LL_DMA_DIRECTION_MEMORY_TO_PERIPH);
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_3, ESC_CMD_BUFFER_LEN);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_3, (uint32_t)ESC_CMD[0]);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_3);


	LL_DMA_DisableStream(DMA2, LL_DMA_STREAM_2);
	LL_DMA_ClearFlag_TC2(DMA2);
	LL_DMA_ClearFlag_DME2(DMA2);
	LL_DMA_ClearFlag_FE2(DMA2);
	LL_DMA_ClearFlag_HT2(DMA2);
	LL_DMA_ClearFlag_TE2(DMA2);
	LL_DMA_SetDataTransferDirection(DMA2, LL_DMA_STREAM_2, LL_DMA_DIRECTION_MEMORY_TO_PERIPH);
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_2, ESC_CMD_BUFFER_LEN);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_2, (uint32_t)ESC_CMD[1]);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_2);

	LL_DMA_DisableStream(DMA2, LL_DMA_STREAM_6);
	LL_DMA_ClearFlag_TC6(DMA2);
	LL_DMA_ClearFlag_DME6(DMA2);
	LL_DMA_ClearFlag_FE6(DMA2);
	LL_DMA_ClearFlag_HT6(DMA2);
	LL_DMA_ClearFlag_TE6(DMA2);
	LL_DMA_SetDataTransferDirection(DMA2, LL_DMA_STREAM_6, LL_DMA_DIRECTION_MEMORY_TO_PERIPH);
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_6, ESC_CMD_BUFFER_LEN);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_6, (uint32_t)ESC_CMD[2]);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_6);

	LL_DMA_DisableStream(DMA2, LL_DMA_STREAM_4);
	LL_DMA_ClearFlag_TC4(DMA2);
	LL_DMA_ClearFlag_DME4(DMA2);
	LL_DMA_ClearFlag_FE4(DMA2);
	LL_DMA_ClearFlag_HT4(DMA2);
	LL_DMA_ClearFlag_TE4(DMA2);
	LL_DMA_SetDataTransferDirection(DMA2, LL_DMA_STREAM_4, LL_DMA_DIRECTION_MEMORY_TO_PERIPH);
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_4, ESC_CMD_BUFFER_LEN);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_4, (uint32_t)ESC_CMD[3]);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_4);

	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_3);
	LL_TIM_EnableDMAReq_CC1(htimA);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH1);


	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_2);
	LL_TIM_EnableDMAReq_CC2(htimA);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH2);


	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_6);
	LL_TIM_EnableDMAReq_CC3(htimA);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH3);


	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_4);
	LL_TIM_EnableDMAReq_CC4(htimA);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH4);

	LL_TIM_EnableAllOutputs(htimA);

	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;
}

extern "C" void motor_main(void *argument)
{
	motor.motor_Init();
	motor.motor_Start();
	for(;;)
	{
		osSemaphoreAcquire(semMotor,0xffffffff);
		motor.motor_run();
	}
}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
